#ifndef FRANKAROBOTCONTROL_H
#define FRANKAROBOTCONTROL_H

#include "./hardware_interface/Robot/RobotControlInterface.h"
#include "hardware_interface/Motor_API/function.h"
//#include <franka/robot.h>
//#include <franka/gripper.h>
//#include <franka/exception.h>
//#include <franka/robot.h>
//#include <franka/duration.h>
//#include <franka/exception.h>
//#include <franka/model.h>
//#include <franka/rate_limiting.h>
//#include "franka/examples_common.h"

#include <array>
#include <atomic>
#include <cmath>
#include <functional>
#include <iostream>
#include <iterator>
#include <mutex>
#include <thread>
#include <QString>
#include <Eigen/Dense>
#include <string>

//兼容的写法需要的库
#include <ratio>
#include <chrono>
#include <thread>
#include "./include/include_franka/franka/robot.h"
#include "./hardware_interface/hardware_variable.h"
using namespace std::chrono;
class FrankaRobotControl : public RobotControlInterface
{
public:
    FrankaRobotControl(QString Name);
    ~FrankaRobotControl();
    franka::Robot* robot;

    QString RobotName="";
    std::string RobotIP="192.168.65.110";
    int RobotPort=0;//#define SERVER_PORT 8899

    int RunCount=0;
    int TestTick=0;
    bool IfConnected=false;
    bool IfExit=false;
    //bool ExtFollowerIFStartTrack=false;//如果关闭这个，则不执行跟随
    static bool ExtFollowerIFStartTrack;//因为franka的特殊写法，只能适应他这个，做成静态的，否则他死循环的时候无法读写非静态变量
    static bool ExtGetStart;
    static int tick;
    static int command;
    static double command_J0;
    static double command_J1;
    static double command_J2;
    static double command_J3;
    static double command_J4;
    static double command_J5;
    static double command_J6;

    static double testvalueX;
    static double testvalueY;
    static double testvalueZ;



    //franka::Robot robot(franaka_IP);//"192.168.65.110"

    ////////////////////////////////////////机器人数据包////////////////////////////////////////


    double RobotJointPos[6];//6 q target
    double RobotJointVel[6];//6
    double RobotJointAcc[6];//6
    double RobotJointCurrent[6];//6
    double RobotJointTorque[6];//6

    double RobotJointPos_Act[6];//6
    double RobotJointVel_Act[6];//6
    double RobotJointCurrent_Act[6];//6
    double RobotJointTorque_Control[6];//6 I control

    double RobotTCPPos[6];//6
    double RobotTCPVel[6];//6
    double RobotTCPForce[6];//6
    double RobotTCPPos_Act[6];//6
    double RobotTCPVel_Act[6];//6


    ////////////////////////////////////////机器人数据包////////////////////////////////////////



    ////////////////////////////////////////主从跟随测试相关变量////////////////////////////////////////


    ////////////////////////////////////////主从跟随测试相关变量////////////////////////////////////////


    bool robotLogin(QString addr, int port);
    bool robotLogout();

    void robotSendCommand(QString cmd);

    //void robotForward(Eigen::Matrix4d& EndMatrix, LREALArray10 robot_current_joint);
    //void robotInverse(Eigen::Matrix3d orientation_matrix, Eigen::Vector3d transposition, LREALArray10 joint_current, LREALArray10 joint_move);

    void robotForward(double jointangle[6], Eigen::Matrix3d& robot_ori, Eigen::Vector3d& robot_trans);
    void robotInverse(Eigen::Matrix4d trans_matrix, double* joint_target);


    void robotMoveJ(LREALArray10 pos);
    void robotMoveL(Eigen::Matrix4d desired_EndMatrix);

    Eigen::Matrix4d robotGetRealtimeDATA();
    SRobotRealTimeDataV2 robotGetRealtimeDATAV2();

    void robotFollowSet(RobotFollowPara followparameter);
    void robotFollowStart();
    void robotFollowStop();
    void robotFollowSetTESTX(double newvalue);
    void robotFollowSetTESTY(double newvalue);
    void robotFollowSetTESTZ(double newvalue);

    void robotFollowSetJoint(LREALArray10 targetjointpos);
    void robotFollowSetTCP(Eigen::Matrix4d desired_EndMatrix);



    bool ifRCMfixed=false;



    void run_code();
private slots:


public slots:
    void runSomeBigWork0(); //no RCM
    void runSomeBigWork1(); //RCM with slide
    void runSomeBigWork2(); //RCM with sigma
    void runSomeBigWork3(); //RCM with camera
    void runSomeBigWork4(); //RCM realtime change

    void runJointWork1(); //Joint change
    void runJointWork2(); //J Joint
    void runJointWork3(); //J Joint by multi_
};

#endif // FRANKAROBOTCONTROL_H
